#ifndef PATROLPLANNER_H_
#define PATROLPLANNER_H_

#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_datatypes.h>
#include <string>
using std::string;

#define GOAL_ROT 1.0 // rotation when arriving to a goal position

/**
 * Class used to plan the patrol path between all given locations
 */
class PatrolPlanner {

public:

	/**
	 * Constructor- creates a new object
	 * @param node reference to node handler
	 * @param xLocs x coordinates of all patrol locations
	 * @param yLocs y coordinates of all patrol locations
	 * @param goalTolerance
	 * @throws throws error value if could not initialize service
	 */
	PatrolPlanner(ros::NodeHandle& node, std::vector<float> xLocs,
			std::vector<float> yLocs, const float& goalTolerance) throw (int);

	/**
	 * Destructor
	 */
	virtual ~PatrolPlanner();

	/**
	 * Plans path according to patrol locations
	 * @param initPos robot's inital position
	 * @param patrolPath[out] generated patrol path
	 * @param[out] segmentsCost cost of each segment in patrol path
	 * @param[out] initCost cost from initial position to patrol's starting point
	 */
	void planPatrolPath(const geometry_msgs::Point& initPos,
			std::vector<geometry_msgs::Point>& patrolPath,
			std::vector<float>& segmentsCost, float& initCost);

private:
	ros::NodeHandle& m_node;
	ros::ServiceClient m_client;
	string m_serviceName;
	string m_worldFrame;
	std::vector<float> m_xLocs; // x coordinates of all given patrol locations
	std::vector<float> m_yLocs; // y coordinates of all given patrol locations
	std::vector<geometry_msgs::Point> m_patrolLocs; // list of all given patrol locations
	float m_goalTolerance; // tolerance distance for reaching goal position

	/**
	 * Converts coordinates of patrol locations to positions
	 */
	void cordsToPos();

	/**
	 * Calculates cost of paths between each pair of
	 * patrol locations
	 * @return cost matrix, where (i,j)'th cell holds cost
	 * of path between location i to location j
	 */
	float** calcCostMatrix();

	/**
	 * Finds closest patrol location to given position
	 * @param initPos position to check
	 * @param remainingLocs indices of patrol locations that were not
	 * assigned to the patrol path so far
	 * @param[out] index index of patrol location in remaining locations list
	 * @return index of closest patrol location
	 */
	int closestPatrolLoc(const geometry_msgs::Point& initPos,
			std::vector<int>& remainingLocs, int& index);

	/**
	 * Finds closest patrol location to patrol location
	 * (which is not the given one)
	 * @param costMatrix paths cost between all pairs of patrol locations
	 * @param srcLocIndex index of source patrol location - must not be in remaining locations list
	 * @param remainingLocs indices of patrol locations that were not
	 * assigned to the patrol path so far
	 * @param[out] index index of patrol location in remaining locations list
	 * @return index of closest patrol location
	 */
	int closestPatrolLoc(float** const costMatrix, const int& srcLocIndex,
			std::vector<int>& remainingLocs, int& index);

	/**
	 * Calculates cost of path between given positions
	 * @param srcPos source position
	 * @param goalPos goal position
	 * @return cost of path between given positions
	 */
	float getPathCost(const geometry_msgs::Point& srcPos,
			const geometry_msgs::Point& goalPos);

	/**
	 * Sorts x and y coordinates of the patrol locations according
	 * to given indices list
	 * @param patrolIndices indices of final patrol path locations
	 * @param[out] patrolPath generated patrol path
	 */
	void genPath(std::vector<int> patrolIndices,
			std::vector<geometry_msgs::Point>& patrolPath);

	/**
	 * Calculates cost of each patrol path segment
	 * @param patrolIndices indices of final patrol path locations
	 * @param costMatrix paths cost between all pairs of patrol locations
	 * @param[out] segmentsCost cost of each segment in patrol path
	 */
	void calcSegmentsCost(float** const costMatrix,
			std::vector<int> patrolIndices, std::vector<float>& segmentsCost);

	/**
	 * Sets values for make-plan service request for the
	 * desired path
	 * @param srcPos source position
	 * @param goalPos goal position
	 * @param request request for make-plan service
	 */
	void fillPathRequest(const geometry_msgs::Point& srcPos,
			const geometry_msgs::Point& goalPos,
			nav_msgs::GetPlan::Request &request);

	/**
	 * Returns a tf::Vector3 object initialized with given position
	 * @param position position to convert
	 * @return tf::Vector initialized with given position's values
	 */
	tf::Vector3 positionToVec(const geometry_msgs::Point& position);

	/**
	 * Converts given angle (degrees) to quaternion
	 * @param angle angle to convert
	 * @return quaternion matching given angle
	 */
	geometry_msgs::Quaternion angleToQuat(const float& angle);
};

#endif /* PATROLPLANNER_H_ */
